#!/usr/bin/env python
# -*- coding: utf-8 -*-
# returns HSV value of the pixel under the cursor in a video stream
# author: achuwilson
# achuwilson.wordpress.com
import cv
import time
import roslib
roslib.load_manifest('tutorialROSOpenCV')
import sys
import rospy
import cv
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

x_co = 0
y_co = 0
def on_mouse(event,x,y,flag,param):
  global x_co
  global y_co
  if(event==cv.CV_EVENT_MOUSEMOVE):
    x_co=x
    y_co=y
def callback(data):
  font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.5, 1, 0, 2, 8)
  cv.NamedWindow("camera", 1)
  bridge=CvBridge()
  src=bridge.imgmsg_to_cv(data, "bgr8")
  cv.Smooth(src, src, cv.CV_BLUR, 3)
  hsv = cv.CreateImage(cv.GetSize(src), 8, 3)
  thr = cv.CreateImage(cv.GetSize(src), 8, 1)
  cv.CvtColor(src, hsv, cv.CV_BGR2HSV)
  cv.SetMouseCallback("camera",on_mouse, 0);
  s=cv.Get2D(hsv,y_co,x_co)
  print "H:",s[0],"      S:",s[1],"       V:",s[2]
  cv.PutText(src,str(s[0])+","+str(s[1])+","+str(s[2]), (x_co,y_co),font, (55,25,255))
  cv.ShowImage("camera", src)
  cv.WaitKey(10)


rospy.init_node('image_converter', anonymous=True)
image_sub = rospy.Subscriber("/camera/image_raw",Image,callback)
try:
  rospy.spin()
except KeyboardInterrupt:
  print "Shutting down"
cv.DestroyAllWindows()



